
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       surface_tracking.c
  * @author     baiyang
  * @date       2023-2-11
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"
#include "surface_tracking.h"

#include <sensor_rangefinder/sensor_rangefinder.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// update_surface_offset - manages the vertical offset of the position controller to follow the measured ground or ceiling
//   level measured using the range finder.
void surftrack_update_surface_offset(surface_tracking_t surftrack)
{
#if RANGEFINDER_ENABLED == ENABLED
    // check for timeout
    const uint32_t now_ms = time_millis();
    const bool timeout = (now_ms - surftrack->last_update_ms) > SURFACE_TRACKING_TIMEOUT_MS;

    // check tracking state and that range finders are healthy
    if (((surftrack->surface == SURFACE_TYPE_GROUND) && fms_rangefinder_alt_ok() && (fms.rangefinder_state.glitch_count == 0)) ||
        ((surftrack->surface == SURFACE_TYPE_CEILING) && fms_rangefinder_up_ok() && (fms.rangefinder_up_state.glitch_count == 0))) {

        // calculate surfaces height above the EKF origin
        // e.g. if vehicle is 10m above the EKF origin and rangefinder reports alt of 3m.  curr_surface_alt_above_origin_cm is 7m (or 700cm)
        RangeFinderState *rf_state = (surftrack->surface == SURFACE_TYPE_GROUND) ? &fms.rangefinder_state : &fms.rangefinder_up_state;

        // update position controller target offset to the surface's alt above the EKF origin
        posctrl_set_pos_offset_target_z_cm(fms.pos_control, rf_state->terrain_offset_cm);
        surftrack->last_update_ms = now_ms;
        surftrack->valid_for_logging = true;

        // reset target altitude if this controller has just been engaged
        // target has been changed between upwards vs downwards
        // or glitch has cleared
        if (timeout ||
            surftrack->reset_target ||
            (surftrack->last_glitch_cleared_ms != rf_state->glitch_cleared_ms)) {
            posctrl_set_pos_offset_z_cm(fms.pos_control, rf_state->terrain_offset_cm);
            surftrack->reset_target = false;
            surftrack->last_glitch_cleared_ms = rf_state->glitch_cleared_ms;
        }

    } else {
        // reset position controller offsets if surface tracking is inactive
        // flag target should be reset when/if it next becomes active
        if (timeout) {
            posctrl_set_pos_offset_z_cm(fms.pos_control, 0);
            posctrl_set_pos_offset_target_z_cm(fms.pos_control, 0);
            surftrack->reset_target = true;
        }
    }
#else
    posctrl_set_pos_offset_z_cm(fms.pos_control, 0);
    posctrl_set_pos_offset_target_z_cm(fms.pos_control, 0);
#endif
}

// get target altitude (in cm) above ground
// returns true if there is a valid target
bool surftrack_get_target_alt_cm(surface_tracking_const_t surftrack, float *target_alt_cm)
{
    // fail if we are not tracking downwards
    if (surftrack->surface != SURFACE_TYPE_GROUND) {
        return false;
    }
    // check target has been updated recently
    if (time_millis() - surftrack->last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
        return false;
    }
    *target_alt_cm = (posctrl_get_pos_target_z_cm(fms.pos_control) - posctrl_get_pos_offset_z_cm(fms.pos_control));
    return true;
}

// set target altitude (in cm) above ground
void surftrack_set_target_alt_cm(surface_tracking_t surftrack, float _target_alt_cm)
{
    // fail if we are not tracking downwards
    if (surftrack->surface != SURFACE_TYPE_GROUND) {
        return;
    }
    posctrl_set_pos_offset_z_cm(fms.pos_control, ahrs_get_position_z_up_cm(fms.ahrs) - _target_alt_cm);
    surftrack->last_update_ms = time_millis();
}

bool surftrack_get_target_dist_for_logging(surface_tracking_const_t surftrack, float *target_dist)
{
    if (!surftrack->valid_for_logging || (surftrack->surface == SURFACE_TYPE_NONE)) {
        return false;
    }

    const float dir = (surftrack->surface == SURFACE_TYPE_GROUND) ? 1.0f : -1.0f;
    *target_dist = dir * (posctrl_get_pos_target_z_cm(fms.pos_control) - posctrl_get_pos_offset_z_cm(fms.pos_control)) * 0.01f;
    return true;
}

float surftrack_get_dist_for_logging(surface_tracking_const_t surftrack)
{
    return ((surftrack->surface == SURFACE_TYPE_CEILING) ? fms.rangefinder_up_state.alt_cm : fms.rangefinder_state.alt_cm) * 0.01f;
}

// set direction
void surftrack_set_surface(surface_tracking_t surftrack, enum SurfaceType new_surface)
{
    if (surftrack->surface == new_surface) {
        return;
    }
    // check we have a range finder in the correct direction
    if ((new_surface == SURFACE_TYPE_GROUND) && !sensor_rngfnd_has_orientation(ROTATION_PITCH_270)) {
        gcs_send_text(MAV_SEVERITY_WARNING, "SurfaceTracking: no downward rangefinder");
        notify_events.user_mode_change_failed = 1;
        return;
    }
    if ((new_surface == SURFACE_TYPE_CEILING) && !sensor_rngfnd_has_orientation(ROTATION_PITCH_90)) {
        gcs_send_text(MAV_SEVERITY_WARNING, "SurfaceTracking: no upward rangefinder");
        notify_events.user_mode_change_failed = 1;
        return;
    }
    surftrack->surface = new_surface;
    surftrack->reset_target = true;
}

/*------------------------------------test------------------------------------*/


